#include<iostream>
#include<vector>
#include<queue>
#include<cmath>
#include<algorithm>
using namespace std;
const int dx[4] = { -1,1,0,0 };
const int dy[4] = { 0,0,-1,1 };
struct Node {
	int x, y;
	float g, h;
	Node* parent;
	Node(int x, int y, float g = 0, float h = 0, Node* parent = nullptr) :x(x), y(y), g(g), h(h), parent(parent) {};
	float f()const
	{
		return g + h;
	}
	bool operator>(const Node& oth)const
	{
		return f() > oth.f();
	}
};
float dis(pair<int, int> f, pair<int, int> s)
{
	return abs(f.first - s.first) + abs(f.second + s.second);
}
bool isvalid(int newx, int newy,vector<vector<int>> grid)
{
	if (newx >= 0 && newx < grid.size() && newy >= 0 && newy < grid[0].size())
		return true;
	else
		return false;
}
vector<pair<int,int>> astar(pair<int, int>& start,pair<int, int>& end,vector<vector<int>>& grid)
{
	priority_queue<Node, vector<Node, greater<Node>>> openlist;
	vector<vector<bool>> closedlist(grid.size(),vector<bool>(grid[0].size(),false));
	Node* startnode = new Node(start.first, start.second, 0, dis(start, end));
	openlist.push(*startnode);
	while (!openlist.empty())
	{
		Node current = openlist.top();
		openlist.pop();
		if (current.x == end.first && current.y == end.second)
		{
			vector<pair<int, int>> path;
			Node* temp = &current;
			while (temp)
			{
				path.push_back({ temp->x,temp->y });
				temp=temp->parent;
			}
			reverse(path.begin(), path.end());
			return path;
		}
		closedlist[current.x][current.y] = true;
		for (int i = 0; i < 4; i++)
		{
			int newx = current.x + dx[i];
			int newy = current.y + dy[i];
			if (isvalid(newx, newy, grid))
			{
				float newg = current.g + 1;
				pair<int, int> tt = { current.x,current.y };
				float newh = dis(tt, end);
				Node* neighbour =new Node(newx, newy, newg, newh, new Node(current));
				openlist.push(*neighbour);
			}
		}



	}
	return {};

}

int main()
{
	vector<vector<int>> grid = {
		{0,1,1,1,0,1},
		{1,1,0,0,0,0},
		{0,1,1,0,0,0},
		{0,1,1,1,0,1},
		{1,1,0,0,0,0},
		{0,1,1,0,0,0},
	};
	pair<int, int> start = { 1,0 };
	pair<int, int> end = { 4,5 };
	vector<pair<int,int>> path = astar(start, end, grid);


	return 0;
}
